import serial 
import time
import camera
# import CSP

ser = serial.Serial(port='com5',baudrate=38400,timeout=0.5)

left_pwm = 0
right_pwm = 0


aim_speed = 1
aim_angle = 90

speed_p = 10
speed_i = 1

angle_p = 10
angle_i = 1

time_each_loop = 20

def GetArdunaldata():
    
    back = None
    speed = 0
    getstr = ser.read_all().decode("utf-8")
    print(getstr)
    strpart = getstr.split('\r\n')
    #a = 
    # #b= 
    # left_speed= int(strpart[0].split(' ')[0])
    # right_speed= int(strpart[0].split(' ')[1])

    # sp = str1.split('\t\n')
    # sp_use = sp[-2]
    # speed = float(sp_use)/100

    # speed = [left_speed,right_speed]
    speed = [0,0]
    # speed = float(str_speed)
    return getstr, speed

def GetCPUdata():
    angle_error,distance_error = 0,0
    # csp.main()
    return angle_error,distance_error

def ControlAlgorithm(speed,distance_errIor,angle_error):
    global left_pwm,right_pwn  
    
    left_speed, right_speed = 0,0
    # speed_output = distance_error * speed_p
    # angle_output = angle_error * angle_p

    # left_pwm = speed + .  + angle_error
    # right_pwm = speed + speed_output - angle_error
    # print('distance_errIor')
    # print(distance_errIor)
    if distance_errIor > -200 and distance_errIor < 200:
        if angle_error > 3:
            left_aim,right_aim = 0,100
        elif angle_error < -3:
            left_aim,right_aim = 100,0
        else:
            left_aim,right_aim = 0,0
        print(distance_errIor)
        if distance_errIor > 0:
            left_aim,right_aim = left_aim+100, right_aim+100

        left_error = left_aim-lelf_speed
        right_error = right_aim -right_sepeed

        left_pwm += left_error/2
        right_pwm += right_error/2

    else:
        left_pwm,right_pwn = 0,0

    #if 
    return left_pwm,right_pwn

def SendArdunaldata(left_pwm,right_pwn):
    # ser.write("c".encode('utf-8'))
    # ser.write(left_pwm.encode('utf-8'))
    # ser.write(right_pwn.encode('utf-8'))

    a =  int(left_pwm/100*250)
    b =  int(right_pwn/100*250)

    sss = '{}{}{}{}{}{}n'.format(a//100,a//10%10,a%10,b//100,b//10%10,b%10).encode('utf-8')
    # print("sss",sss)
    ser.write(sss)
    # ser.write('080080\t\n'.encode('utf-8'))

    return sss

def main_speed():
    time.sleep(0.1)
    while 1:
        
        # 记录当前时间
        time1 = (int(round(time.time() * 1000)))
        getstr,speed = GetArdunaldata()
        
        current_speed = 0
        # 读取距离和角度
        current_distance,current_angle = camera.redRecognition()
        
        error_angle = current_angle
        error_distance = current_distance-100 
        
        # 生成控制信号
        left_pwm,right_pwn = ControlAlgorithm(current_speed,error_distance,error_angle)
        # 发送控制信号
        send = SendArdunaldata(left_pwm,right_pwn)
        
        # 记录当前时间
        time2 = (int(round(time.time() * 1000)))

        # 时间控制
        time_use = time2-time1
        time_sleep = time_each_loop - time_use
        time.sleep(time_sleep/1000)

        print('get:{}\t send:{}\t time_use:{}\t speed:{}\t angle:{}\t dis:{}\t left_pwm:{}\t right_pwm:{}'.format(getstr,send, time_use,current_speed,current_angle,current_distance ,left_pwm,right_pwn))

    ser.close()  


def main():
    time.sleep(0.1)
    while 1:
        
        # 记录当前时间
        time1 = (int(round(time.time() * 1000)))
        getstr,speed = GetArdunaldata()
        
        current_speed = 0
        # 读取距离和角度
        current_distance,current_angle = camera.redRecognition()
        
        error_angle = current_angle
        error_distance = current_distance-100 
        
        # 生成控制信号
        left_pwm,right_pwn = ControlAlgorithm(current_speed,error_distance,error_angle)
        # 发送控制信号
        send = SendArdunaldata(left_pwm,right_pwn)
        
        # 记录当前时间
        time2 = (int(round(time.time() * 1000)))

        # 时间控制
        time_use = time2-time1
        time_sleep = time_each_loop - time_use
        time.sleep(time_sleep/1000)

        print('get:{}\t send:{}\t time_use:{}\t speed:{}\t angle:{}\t dis:{}\t left_pwm:{}\t right_pwm:{}'.format(getstr,send, time_use,current_speed,current_angle,current_distance ,left_pwm,right_pwn))

    ser.close()  

def PutPwm():
    pwma = 33 # 百分比
    pwmb = 33

    time.sleep(0.1)
    while 1:
        
        # 记录当前时间
        time1 = (int(round(time.time() * 1000)))

        # 从mcu读取数据
        getstr,speed = GetArdunaldata()
        current_speed = 0
        # current_distance,current_angle = camera.redRecognition()
        current_distance,current_angle = 0,0
        error_angle = current_angle
        error_distance = current_distance-100 
        
        # left_pwm,right_pwn = ControlAlgorithm(current_speed,current_distance,current_angle)
        left_pwm,right_pwn = ControlAlgorithm(current_speed,error_distance,error_angle)
        send = SendArdunaldata(pwma,pwmb)
        time2 = (int(round(time.time() * 1000)))

        time_use = time2-time1
        time_sleep = time_each_loop - time_use
        time.sleep(time_sleep/1000)

        print('get:{}\t send:{}\t time_use:{}\t speed:{}\t angle:{}\t dis:{}\t left_pwm:{}\t right_pwm:{}'.format(getstr,send, time_use,current_speed,current_angle,current_distance ,left_pwm,right_pwn))

    ser.close()  

if __name__ == '__main__':
    PutPwm()
    # main()

